package Robot;
import Util.Pose2d;
import javaclient2.*;
import javaclient2.structures.PlayerConstants;
import javaclient2.structures.PlayerPose;


public class RealPosition2d  extends Position2d{

    
    private Position2DInterface position=null;
    

    /**
     *Constructor of the class
     */
    public RealPosition2d(Robot robot, String name, int index){
        
        //constructor of father-class
        super(robot, name, index);
        
        
        
            
        position=robot.getConnection().requestInterfacePosition2D(deviceIndex, PlayerConstants.PLAYER_OPEN_MODE);

        if(position!=null)
            System.out.println("Interface position2d:"+
                                Integer.toString(deviceIndex)+
                                    " of Robot:"+
                                        Integer.toString(robot.getId())+" Ok");
        else{
            System.out.println("Interface position2d:"+
                                Integer.toString(deviceIndex)+
                                    " of Robot:"+
                                        Integer.toString(robot.getId())+" Fail");
            return;
        }

        this.setPlayerDevice(position);


        data=new Pose2d(0f,0f,0f);
        /*Cuando se llama a esta funcion "setMotorPower", se tienen problemas posteriormente, en este caso
         * se tienen problemas cuando se crea otra interfaz (EJ: Sonar), ya que no recupera la interfaz que 
         * es. Al parecer, la llamada a setMotorPower, se debe realizar posterior a la creacion de las interfaces
         * y a la ejecucion del metodo runThreaded.
         */
        //position.setMotorPower(1);
        //wait until update was succesfull
        //while(!update());
    }


    /**
     *
     * Define si se puede leer o no
     */
    
    public void setMotorPower(boolean state)
    {
        if(state)
            position.setMotorPower(1);
        else
            position.setMotorPower(0);
        
    }
    
    public boolean isReady()
    {
       return position.isDataReady();
    }
    /**
     *
     * Pos inicial
     */
    public void setInitialPosition(Pose2d initial){

        position.resetOdometry();
        position.setOdometry(initial);
        initial.copy(data);
    }
    
    /**
     *
     * Pos inicial
     */
    public boolean update(){
        if(isReady()){
            //System.out.println("data ready on Position2d");
            data.setPa(position.getData().getPos().getPa());
            data.setPx(position.getData().getPos().getPx());
            data.setPy(position.getData().getPos().getPy());
            
            //data.add(initialPosition);
            return true;
        }
        else
            return false;
           
    
    }
    
    
     
    public void setSpeed(float val1, float val2){
//        PlayerPose vel=new  PlayerPose();
//        vel.setPx(val1);
//        vel.setPy(0.0f);
//        vel.setPa(val2);
        position.setSpeed(val1, val2);
        //position.setVelocity(vel,0);
    }
    

    
}
